/*============================================================================*/
/*                        Tortoise Team			                              */
/*============================================================================*/
/*                        OBJECT SPECIFICATION                                */
/*============================================================================*
* C Source:         %APP_Linear_Camera.c%
* Instance:         RPL_1
* %version:         1.2 %
* %created_by:      Andres Torres Garcia %
* %date_created:    Sunday Sep  30 14:38:03 2012 %
*=============================================================================*/
/* DESCRIPTION : C source file for the application layer of the Linear Camera */
/*============================================================================*/
/* FUNCTION COMMENT : This file contains the declarations and definitions of  */
/*					  the variables, and functions of the application layer of*/													  
/*					  the camera.											  */ 
/*					  	  											  		  */
/* 					                      									  */
/*                                                                            */
/*============================================================================*/
/*                               OBJECT HISTORY                               */
/*============================================================================*/
/*  REVISION |   DATE      |                               |      AUTHOR      */
/*----------------------------------------------------------------------------*/
/*  1.0      | 30/09/2012  |                               | Andres Torres    */
/*  1.1      | 04/10/2012  |                               | Andres Torres    */
/*  1.2      | 07/10/2012  |                               | Andres Torres    */
/* Integration under Continuus CM                                             */
/*============================================================================*/

/* Includes */
/* -------- */
#include "APP_Linear_Camera.h"
#include "HAL_Linear_Camera.h"

/* Functions macros, constants, types and datas         */
/* ---------------------------------------------------- */
/* Functions macros */

#define MIN_PIX_LINE	5

#define PAT_LENGHT	10

/*Change this, depending in the camera position.*/
#define GOAL		42	//0010-1010
#define BLACK_LINE	 2	//0000-0010

#define PIN_DEBUG	1

/*==================================================*/ 
/* Definition of constants                          */
/*==================================================*/ 

const T_UBYTE ub_RANGE = 4;


/*======================================================*/ 
/* Definition of RAM variables                          */
/*======================================================*/

typedef struct Pattern_info {
	
	T_UBYTE width;
	T_UBYTE leftBorder;
	T_UBYTE rightBorder;
	
} Pattern_info;

Pattern_info pat_info[PAT_LENGHT];

static T_UBYTE ub_MinPixPatt = 2;//ub_BLACK_STRIP_WIDTH_MEASU / 2;

extern T_UWORD uw_Pixels[NUM_PIXELS];

T_UBYTE ub_Bin_Pixels[NUM_PIXELS];

T_UBYTE ub_Line_Average[NUM_PIXELS];

T_UBYTE ub_Left_Corner = 0;
T_UBYTE ub_Right_Corner = 0;

T_UBYTE ub_Black_Strip_Width = 0;
T_UBYTE ub_Black_Strip_Center = 0;

T_UBYTE ub_Horizontal_Lines = 0;

T_UBYTE Can_Process_Pixels = 0;
T_UWORD Threshold = 200;//500;

T_UBYTE ub_ArePixRdy = 0;

T_UBYTE ub_IsGoal = 0;

static T_UBYTE ub_BLACK_STRIP_WIDTH_MEASU = 20;

/*======================================================*/ 
/* close variable declaration sections                  */
/*======================================================*/ 

/* Private defines */

/* Private functions prototypes */
/* ---------------------------- */
void vfn_saveLineCenterPosition(void);
T_UBYTE detectLine(void);

/* Exported functions prototypes */
/* ----------------------------- */

void vfnProcess_Pixels(void);
void vfnCalibrateLine(void);
void vfnCalibrate_Linear_Camera(void);
void vfnInterrupToProc_And_CaptPix(void);

/* Inline functions */
/* ---------------- */

/* Private functions */
/* ----------------- */
/**************************************************************
 *  Name                 : 	vfn_saveLineCenterPosition
 *  Description          :	Store different reads of the line center in an array.
 *  Parameters           :  [Input, Output, Input / output]
 *  Return               :	void
 *  Critical/explanation :    No
 **************************************************************/
 
 void vfn_saveLineCenterPosition(void)
 {
 	T_UBYTE i;

	for(i = CAMERA_AVG; i > 0; i--)
	{
		ub_Line_Average[i] = ub_Line_Average[i-1];
	}	
 }

/* Exported functions */
/* ------------------ */
/**************************************************************
 *  Name                 : 	vfnProcess_Pixels
 *  Description          :	Process the pixels of the linear camera and detect the line
 *  Parameters           :  [Input, Output, Input / output]
 *  Return               :	void
 *  Critical/explanation :    No
 **************************************************************/

void vfnProcess_Pixels(void)
{
	T_UBYTE i;
	T_UBYTE type_pat = 0;
	T_UBYTE option = 0;

	for(i = 0; i < NUM_PIXELS; i++)
	{
		if(uw_Pixels[i] > Threshold)
		{
			ub_Bin_Pixels[i] = 0;
		}
		else
		{
			ub_Bin_Pixels[i] = 1;
		}
	}
	
	vfn_saveLineCenterPosition();
	
	ub_IsGoal = 0;
	
	if( type_pat = detectLine() )
	{
		if(1 == type_pat)
		{
			option = 1;
		}
		else if(2 == type_pat)
		{
			option = 3;
		}
		
		//TODO: Check later if this if is necessary
		if( ( ( ub_BLACK_STRIP_WIDTH_MEASU - ub_RANGE ) <= pat_info[option].width ) /*&& ( ( ub_BLACK_STRIP_WIDTH_MEASU + ub_RANGE ) >= pat_info[option].width )*/ )
        {
        	ub_IsGoal = type_pat - 1;
			ub_Black_Strip_Width = pat_info[option].width;
			ub_Right_Corner = pat_info[option].rightBorder;
			ub_Left_Corner = pat_info[option].leftBorder;
		
			ub_Line_Average[0] = ub_Black_Strip_Center = (pat_info[option].width / 2) + pat_info[option].leftBorder;
        }	
	}
	
	ub_ArePixRdy = 1;
}

/**************************************************************
 *  Name                 : 	vfnCalibrate_Linear_Camera
 *  Description          :	Calculate the threshold automatically
 *  Parameters           :  [Input, Output, Input / output]
 *  Return               :	void
 *  Critical/explanation :    No
 **************************************************************/

void vfnCalibrate_Linear_Camera(void)
{
	T_UWORD uw_minValue = 0x400;
	T_UWORD uw_maxValue = 0x0;
	T_UWORD uw_difference = 0x0;
	
	T_UBYTE i;
	
	for(i = 0; i < NUM_PIXELS; i++)
	{
		if(uw_Pixels[i] > uw_maxValue)
		{
			uw_maxValue = uw_Pixels[i];
		}
		
		if(uw_Pixels[i] < uw_minValue)
		{
			uw_minValue = uw_Pixels[i];
		}
	}
	
	uw_difference = uw_maxValue - uw_minValue;
	
	Threshold = (uw_difference / 2) + uw_minValue;
}

/**************************************************************
 *  Name                 : 	vfnInterrupToProcPix
 *  Description          :	Interruption to proccess the pixels of the linear camera.
 *  Parameters           :  [Input, Output, Input / output]
 *  Return               :	void
 *  Critical/explanation :    No
 **************************************************************/
void vfnInterrupToProc_And_CaptPix(void)
{
	vfnInterrupToCapturePix();
	
	if(Can_Process_Pixels)
	{
		//IO_ToggleOutput(PIN_DEBUG);
		vfnProcess_Pixels();
		//IO_ToggleOutput(PIN_DEBUG);
	}
}

/**************************************************************
 *  Name                 : 	detectLine
 *  Description          :	Try to detect the line.
 *  Parameters           :  [Input, Output, Input / output]
 *  Return               :	T_UBYTE 2: goal, 1: black line, 0: no pattern detected.
 *  Critical/explanation :    No
 **************************************************************/
T_UBYTE detectLine(void)
{
	T_UBYTE ub_i;
	T_UBYTE ub_pattern = 0;
	T_UBYTE ub_j = 0;
	T_UBYTE ub_value = 0;
	T_UBYTE ub_line_width = 0;
	
	for(ub_i = 0; ub_i < NUM_PIXELS;)
	{
		ub_value = ub_Bin_Pixels[ub_i];
		
		while( ( ub_value == ub_Bin_Pixels[ub_i] ) && ( ub_i < NUM_PIXELS ) )
		{
			ub_line_width++;
			ub_i++;
		}
		
		if( ub_line_width >= ub_MinPixPatt )
		{	
			ub_pattern = ub_pattern << 1;
			ub_pattern |= ub_value;
			
			pat_info[ub_j].width = ub_line_width;
			pat_info[ub_j].leftBorder = ub_i - ub_line_width;
			pat_info[ub_j++].rightBorder = ub_i - 1;
		}
		
		ub_line_width = 0;
	}
	
	if( ub_pattern == BLACK_LINE )
	{
		return 1;
	}
	else if( ub_pattern == GOAL )
	{
		return 2;
	}
	
	return 0;
}

/**************************************************************
 *  Name                 : 	vfnCalibrateLine
 *  Description          :	Measure the line in pixels
 *  Parameters           :  [Input, Output, Input / output]
 *  Return               :	void
 *  Critical/explanation :    No
 **************************************************************/
void vfnCalibrateLine(void)
{
	T_UBYTE i;
	T_UWORD uw_min = 0x400;
	static T_UBYTE ub_min_pos = 0;

	for(i = 0; i < NUM_PIXELS; i++)
	{
		if(uw_Pixels[i] > Threshold)
		{
			ub_Bin_Pixels[i] = 0;
		}
		else
		{
			ub_Bin_Pixels[i] = 1;
		}
		
		if(uw_min > uw_Pixels[i])
		{
			uw_min = uw_Pixels[i];
			ub_min_pos = i;
		}
	}
	
	ub_Left_Corner = ub_Right_Corner = ub_min_pos;
	
	while( ( 0 != ub_Left_Corner ) && ( ub_Bin_Pixels[ub_Left_Corner - 1] == ub_Bin_Pixels[ub_Left_Corner] ) )
	{
		ub_Left_Corner--;
	}
	
	while( ( (NUM_PIXELS - 1) != ub_Right_Corner ) && ( ub_Bin_Pixels[ub_Right_Corner + 1] == ub_Bin_Pixels[ub_Right_Corner] ) )
	{
		ub_Right_Corner++;
	}
	
	ub_BLACK_STRIP_WIDTH_MEASU = (ub_Right_Corner - ub_Left_Corner) + 1;
}